from PySide6.QtWidgets import QWidget,QApplication,QFileDialog
from PySide6.QtSerialPort import QSerialPortInfo,QSerialPort
from PySide6.QtCore import Slot,QFile,QTimer
from PySide6.QtGui import QImage,QPixmap
from PySide6.QtNetwork import QTcpSocket
from ui_controller import Ui_Form
from math import sqrt
import cv2

# 定义主窗体类
class ControllerWin(QWidget):
    def __init__(self):
        # 调用QWidget的构造函数，初始化主窗体
        super().__init__()
        # 创建ui对象
        self.ui = Ui_Form()
        # 在主窗体上绘制控件
        self.ui.setupUi(self)
        # 列出系统中所有串口设备
        portList = QSerialPortInfo.availablePorts()
        for port in portList:
            # 向下拉列表中增加选项
            self.ui.comboBox.addItem(port.portName())
        
        # 添加TCP客户端选项
        self.ui.comboBox.addItem("TCP")

        # 关联连接按钮的点击信号和回调函数（槽函数）
        self.ui.connectButton.clicked.connect(self.connectButtonClicked)
        self.ui.closeButton.clicked.connect(self.closeButtonClicked)
        self.ui.sendButton.clicked.connect(self.sendButtonClicked)
        self.ui.clearButton.clicked.connect(self.clearButtonClicked)
        self.ui.runButton.clicked.connect(self.runButtonClicked)
        self.ui.openButton.clicked.connect(self.openButtonClicked)
        # 创建串口对象
        self.serial = QSerialPort()
        # 创建TCP客户端对象
        self.client = QTcpSocket()
        # 没有连接时需要禁用发送和关闭按钮
        self.ui.closeButton.setDisabled(True)
        self.ui.sendButton.setDisabled(True)
        # 关联串口接收的回调函数
        self.serial.readyRead.connect(self.recvSerialMsg)
        # 关联TCP服务器消息接收的回调函数
        self.client.readyRead.connect(self.recvServerMsg)
        # 关联滑动条修改信号和回调函数
        self.ui.xSlider.valueChanged.connect(self.generateCmd)
        self.ui.ySlider.valueChanged.connect(self.generateCmd)
        self.ui.zSlider.valueChanged.connect(self.generateCmd)
        # 创建文件对象
        self.script = QFile()
        # 创建定时器对象
        self.timer = QTimer()
        self.timer.timeout.connect(self.runScript)
        # 创建摄像头对象
        self.cap = cv2.VideoCapture()
        # 创建视频采集定时器
        self.cap_timer = QTimer()
        self.cap_timer.timeout.connect(self.capture)
        # 创建标签检测器
        aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
        aruco_param = cv2.aruco.DetectorParameters()
        self.aruco_detector = cv2.aruco.ArucoDetector(aruco_dict,aruco_param)
        

    #按键点击回调函数
    @Slot()
    def connectButtonClicked(self):
        # 如果下拉列表选择TCP，则连接TCP服务器，否则连接串口
        if self.ui.comboBox.currentText() == "TCP":
            self.client.connectToHost(self.ui.ipEdit.text(), 6666)
            # 启用发送和关闭按钮
            self.ui.closeButton.setEnabled(True)
            self.ui.sendButton.setEnabled(True)
            # 禁用连接按钮
            self.ui.connectButton.setDisabled(True)
        else:
            # 获取下拉列表选中的串口
            self.serial.setPortName(self.ui.comboBox.currentText())
            # 连接串口
            if self.serial.open(QSerialPort.ReadWrite):
                # 启用发送和关闭按钮
                self.ui.closeButton.setEnabled(True)
                self.ui.sendButton.setEnabled(True)
                # 禁用连接按钮
                self.ui.connectButton.setDisabled(True)

    @Slot()
    def closeButtonClicked(self):
        if self.ui.comboBox.currentText() == "TCP":
            self.client.close()
        else:
            # 关闭串口
            self.serial.close()
        # 禁用发送和关闭按钮，启用连接按钮
        self.ui.closeButton.setDisabled(True)
        self.ui.sendButton.setDisabled(True)
        self.ui.connectButton.setEnabled(True)

    @Slot()
    def sendButtonClicked(self):
        # 将输入框中的文本发送到串口
        self.ui.plainTextEdit.appendPlainText(self.ui.lineEdit.text())
        if self.ui.comboBox.currentText() == "TCP":
            self.client.write(self.ui.lineEdit.text().encode())
            self.client.write("\n".encode())
        else:
            self.serial.write(self.ui.lineEdit.text().encode())
            self.serial.write("\n".encode())

    @Slot()
    def recvSerialMsg(self):
        # 读取串口接收的数据，并显示到文本框中
        # 等待一行结束
        if not self.serial.canReadLine():
            return
        self.ui.plainTextEdit.appendPlainText(self.serial.readLine().data().decode().strip())

    @Slot()
    def recvServerMsg(self):
        # 读取串口接收的数据，并显示到文本框中
        # 等待一行结束
        if not self.client.canReadLine():
            return
        self.ui.plainTextEdit.appendPlainText(self.client.readLine().data().decode().strip())

    @Slot()
    def clearButtonClicked(self):
        # 清除文本框中的内容
        self.ui.plainTextEdit.clear()

    @Slot()
    def generateCmd(self):
        # 当滑动条滑动时自动生成G代码
        cmd = "G00 X" + str(self.ui.xSlider.value()) + " Y" + str(self.ui.ySlider.value()) + " Z" + str(self.ui.zSlider.value())
        self.ui.lineEdit.setText(cmd)

    @Slot()
    def runButtonClicked(self):
        # 打开脚本文件并执行
        path = self.ui.pathEdit.text()
        # 如果没有指定文件名，打开文件对话框
        if path == "":
            path = QFileDialog.getOpenFileName(self)[0]
            self.ui.pathEdit.setText(path)
        # 以只读方式打开文件
        self.script.setFileName(path)
        self.script.open(QFile.ReadOnly)
        # 打开文件后每秒钟从文件中读取一行G代码并发送到机械臂执行
        self.timer.start(1000)

    @Slot()
    def runScript(self):
        # 如果读到脚本的最后，停止执行
        if self.script.atEnd():
            self.timer.stop()
            self.script.close()
            return
        # 从脚本中读取一行代码
        cmd = self.script.readLine().data().decode()
        self.ui.lineEdit.setText(cmd)
        # 发送到机械臂
        self.sendButtonClicked(self)

    @Slot()
    def openButtonClicked(self):
        # 使用VideoCapture打开摄像头，并在窗口上显示捕获到的图像
        self.cap.open(self.ui.addrEdit.text())
        # 获取摄像头的FPS
        fps = self.cap.get(cv2.CAP_PROP_FPS)
        self.cap_timer.start(int(1000/fps))

    @Slot()
    def capture(self):
        # 从摄像头中读取一帧图像，并显示到窗口的LABEL控件中
        success,frame = self.cap.read()
        if success:
            # 检测标签
            corners, ids, rejected = self.aruco_detector.detectMarkers(frame)
            for tag in corners:
                    # 计算标签的最小外接圆，圆心坐标即标签的中心坐标
                    center, radius = cv2.minEnclosingCircle(tag)
                    cv2.circle(frame, (int(center[0]), int(center[1])), int(radius), (0,0,255), 2)
                    #print(330/radius*2)
                    # 外切圆实际长度，单位cm
                    real_r = sqrt(2.0 * 2.0 * 2) / 2
                    # 计算每厘米像素数
                    pxpcm = radius/real_r
                    # 物理坐标，单位cm
                    real_x = int((int(center[0]) - 320)/pxpcm)
                    real_y = int((480 - int(center[1]))/pxpcm)
                    real_z = 30 - int(330/radius*2)
                    #print("中心坐标：X", (int(center[0]) - 320)/pxpcm, " Y", (480 - int(center[1]))/pxpcm, " R", radius)
                    # 根据物理坐标生成G代码
                    cmd = "G00 X" + str(real_x) + " Y" + str(real_y) + " Z" + str(real_z)
                    self.ui.lineEdit.setText(cmd)

            # 绘制标签
            # cv2.aruco.drawDetectedMarkers(frame, corners, ids)
            image = QImage(frame.data, frame.shape[1], frame.shape[0], QImage.Format_BGR888)
            self.ui.capLabel.setPixmap(QPixmap.fromImage(image))

if __name__ == "__main__":
    # 创建应用程序
    app = QApplication()
    # 创建主窗口
    win = ControllerWin()
    # 显示主窗口
    win.show()
    # 处理用户输入
    app.exec()
